#include <nuttx/config.h>
#include <stdio.h>
#include <common/Mutex.h>
#include <common/Semaphore.h>
#include <common/Queue.h>
#include <common/Thread.h>
#include <common/Runnable.h>
#include "zorfcs/filestorage.h"
#include "test.h"

namespace {
void testThread();
void testSemaphore();
void testMutex();
void testQueue();
void testFileStorage();
}

void do_test() {
	printf("Begin of tests.\n");
	testThread();
	testSemaphore();
	testMutex();
	testQueue();
	testFileStorage();

	printf("End of tests.\n");
}

namespace {

class TestRunnable : public Runnable {
public:
	TestRunnable(volatile int &value, Semaphore *sem, Mutex *mut)
	: value_(value), sem_(sem), mut_(mut)
	{}

	virtual void run() {
		while (!isExitFlag()) {
			if (sem_ != NULL) {
				if (!sem_->take(100))
					continue;
			}

			if (mut_ != NULL) {
				if (!mut_->lock(100))
					continue;
				mut_->unlock();
			}
			value_ = 1234;
			Thread::sleep(20);
		}
	}

private:
	volatile int &value_;
	Semaphore *sem_;
	Mutex *mut_;
};



void testThread() {
	printf("Test Thread ");
	volatile int value = 0;
	Thread *thread = new Thread("test thread", 2048, SCHED_PRIORITY_DEFAULT, new TestRunnable(value, NULL, NULL));
	Thread::sleep(100);
	delete thread;
	printf("%s\n", value == 1234 ? "OK" : "FAIL");
}

void testSemaphore() {
	printf("Test Semaphore ");
	Semaphore *sem = new Semaphore(0);
	volatile int value = 0;
	Thread *thread = new Thread("test thread", 2048, SCHED_PRIORITY_DEFAULT, new TestRunnable(value, sem, NULL));
	Thread::sleep(100);
	bool res = value == 0;
	sem->give();
	Thread::sleep(100);
	res &= value == 1234;
	delete thread;
	delete sem;
	printf("%s\n", res ? "OK" : "FAIL");
}

void testMutex() {
	printf("Test Mutex ");
	Mutex *mut = new Mutex();
	mut->lock();
	volatile int value = 0;
	Thread *thread = new Thread("test thread", 2048, SCHED_PRIORITY_DEFAULT, new TestRunnable(value, NULL, mut));
	Thread::sleep(100);
	bool res = value == 0;
	mut->unlock();
	Thread::sleep(100);
	res &= value == 1234;
	delete thread;
	delete mut;
	printf("%s\n", res ? "OK" : "FAIL");
}

void testQueue() {
	printf("Test Queue ");
	Queue *queue = new Queue(16, sizeof(int));
	int v = 1234;
	queue->put(&v);
	v = 2345;
	queue->put(&v);
	bool res = queue->get(&v);
	res &= v == 1234;
	res &= queue->get(&v);
	res &= v == 2345;
	res &= queue->get(&v, 100) == false;
	delete queue;
	printf("%s\n", res ? "OK" : "FAIL");
}

void testFileStorage() {
	printf("Test FileStorage ");
	FileStorage *fs = new FileStorage("/fs/microsd/testfs.bin", 1024);
	fs->lock();
	char data[4] = {'1', '2', '3', '4'};
	fs->write(16, data, sizeof(data));
	data[0] = data[1] = data[2] = data[3] = 0;
	fs->read(16, data, sizeof(data));
	fs->unlock();
	bool res = data[0] == '1' && data[1] == '2' && data[2] == '3' && data[3] == '4';
	delete fs;
	printf("%s\n", res ? "OK" : "FAIL");
}

}
